Learning Objectives
- Explain the concept of sensor fusion algorithm in robotics applications.
- Apply the complementary filter algorithm to fuse compass sensor and wheel encoder data for heading control of a mobile robot.
- Describe the limitation of complimentary filter
Recap
- we have derived the odometry formula for wheeled mobile robot (differential-drive) with encoder input in the previous lecture class.
- The encoder (with pulses input) odometry formula:
- Modify to encoder (with radian input) for Webots simulator
- We have implemented square path movement with the formula above in Webots simulator in the previous tutorial.
# === Odometry Update Function ===
def update_odometry():
global prev_left, prev_right, pose
cur_left = left_sensor.getValue()
cur_right = right_sensor.getValue()
dphi_l = cur_left - prev_left
dphi_r = cur_right - prev_right
prev_left = cur_left
prev_right = cur_right
delta_s = WHEEL_RADIUS * (dphi_r + dphi_l) / 2.0
delta_theta = WHEEL_RADIUS * (dphi_r - dphi_l) / WHEEL_BASE
theta_mid = pose[2] + delta_theta / 2.0
pose[0] += delta_s * math.cos(theta_mid)
pose[1] += delta_s * math.sin(theta_mid)
pose[2] += delta_theta
pose[2] = (pose[2] + math.pi) % (2 * math.pi) - math.pi
return delta_s, delta_theta
- it seems perfect, when we run only 1 lap (4 sides)
-
the odometry reports
-
Suppose it should be x = 0; y = 0; θ = 360°
-
What caused the error?
-
What if we ran more laps?
-
16 laps :
-
the odometry reports:
64 laps:
- the odometry reports:
The problem comes from Angular Drift!!
Using Compass Sensor
-
Add a compass sensor to the robot.
-
-
-
Search for "compass"
-
Getting compass values (Sample Code)
import math
from controller import Robot
compass = robot.getDevice("compass")
compass.enable(TIME_STEP)
def get_heading_angle(compass_values):
rad = math.atan2(compass_values[0], compass_values[1])
if rad < 0:
rad += 2 * math.pi
return rad
def compass_reading():
compass_values = compass.getValues()
current_heading = get_heading_angle(compass_values)
print(f"Compass: {math.degrees(current_heading) % 360:.2f}")
Take note: the Webots simulator's compass has no noise, which never happens in the real world!
- if we run 64 laps in the simulator with compass sensor.
-
the odometry reports:
-
As for orientation:
-
Diff = (360° - 347.8° (from wheel encoder) ) + 0.85° (compass) = 13.05° degree
Let's add some noise to the compass sensor to make it more realistic
def get_heading_angle(compass_values):
noise_std_deg = 2.0
rad = math.atan2(compass_values[0], compass_values[1])
if rad < 0:
rad += 2 * math.pi
noise = math.radians(random.gauss(0, noise_std_deg))
noisy_heading = (rad + noise) % (2 * math.pi)
return noisy_heading
It is what we likely to have in the real world
-
Now, we have two sensors and each of them has their strength and weakness.
-
Compass sensor (with noise):
- Advantage: Error does not accumulate over time
- Disadvantage: Insensitive to small orientation changes
-
Wheel encoder (for orientation):
- Advantage: Sensitive to small orientation changes
- Disadvantage: Error does accumulate over time
It is possible for us to combine them?
Complimentary Filter
A complementary filter is a simple and effective sensor fusion technique that combines two (or more) measurements that complement each other in the frequency domain—one sensor gives reliable high-frequency data, and the other gives reliable low-frequency data.
How It Works (In General)
The core idea is to blend the strengths of two sensors:
- One sensor gives fast but noisy data (good at detecting rapid changes).
- Another gives slow but stable data (good at long-term stability but poor at tracking fast changes).
Mathematically, a complementary filter for combining two signals A(t)A(t)A(t) and B(t)B(t)B(t) can be expressed as:
- α is a tuning constant (e.g., between 0.90 to 0.98),
- A(t) is typically the high-frequency sensor (e.g., gyro or encoder),
- B(t) is typically the low-frequency sensor (e.g., compass or accelerometer).
Using Complementary Filter for Orientation with Compass and Wheel Encoder
- Compass gives absolute heading (yaw) relative to magnetic north. It’s stable in the long term but noisy and affected by magnetic disturbances.
- Wheel encoders give relative changes in orientation (by tracking differential wheel speeds), which are accurate short-term but drift over time due to wheel slip, uneven terrain, or integration errors.
Why It Works
- Trust the wheel encoders for short-term changes (they quickly detect turns).
- Trust the compass for long-term orientation (to correct drift).
The complementary filter might look like:
Where:
-
θencoder: orientation change from encoders
-
θcompass: absolute heading from compass
-
“Trust the encoder for how things have changed.”
-
“Trust the compass for the long-term heading.”
-
Blend the two to get a more reliable heading.
Summary
- Why it works: One sensor's weaknesses are offset by the other's strengths.
- How it works: By weighting and blending sensor readings over time to produce a stable, accurate estimate.
- In your case: Wheel encoders handle quick movements; compass provides slow, stable heading correction.
How do we tune the α?
-
if α is 0, then we trust the compass 100%, and encoder 0%.
-
The result of 16 laps.
-
if α is 1.0, then we trust the compass 0%, and encoder 100%.
-
The result of 16 laps.
-
-
As mentioned earlier, α is a tuning constant (usaully between 0.90 to 0.98)
-
After some trial and error, we found that the best tuning constant for α is 0.94.
-
The result of 16 laps.
-
The result looks promising in this case. However, it is not yet a real-world scenario, as the slippery model of the wheel has not been considered.
The Wheel Slippery Model
- Adding dynamic noise to the wheel sensors
- Assume that angular velocity is proportional to the slipperiness of the wheel.
- Calculate the velocity
# Compute velocities
def update_odometry(dt):
'''
'''
linear_velocity = delta_s / dt
angular_velocity = delta_theta_enc / dt
dt = TIME_STEP / 1000.0 # Convert ms to seconds
_, delta_theta = update_odometry(dt)
- Add the function to inject the dynamic noise into wheel encoder readings
def add_wheel_noise(delta, velocity):
# Add a velocity-correlated bias and Gaussian noise
drift = 0.02 * velocity * random.uniform(-1, 1) # dynamic bias
noise = random.gauss(0, 0.05) # random white noise
return delta + drift + noise
-
let's see the outcome of 16 laps after adding the Wheel Slippery Model with respect to angular velocity.
-
it clearly shows that the complimentary cannot handle the dynamic noise.
-
particularly noise that violates the assumptions of complementary filters. e.g., low-frequency bias drift, correlated or state-dependent noise
-
Complementary filters assume high-frequency noise and no bias. They fail with slow drift or non-Gaussian dynamics like the velocity-correlated drift above.
-
How to solve this problem? we need a better filtering algorithm! The Kalman filter!
Wrapping Up the Section
- Explain the concept of sensor fusion algorithm in robotics applications.
- Apply the complimentary algorithm for compass sensor + wheel encoder for mobile robot orientation.
- Describe the limitation of complimentary filter.